#include "robot_joint.h"

point_t footEnd_point[6];
ik_t joint_ik[6] = {
	{.joint1_angle_err =  30.0f},
	{.joint1_angle_err =  00.0f},
	{.joint1_angle_err = -30.0f},
	
	{.joint1_angle_err =  30.0f},
	{.joint1_angle_err =  00.0f},
	{.joint1_angle_err = -30.0f},
};

/**
 * @brief  机器人初始化
 * @param  
 * @retval 无
 */
void robot_joint_init(void)
{
	for (uint8_t i=0; i < 6; i++) {
		footEnd_point[i].x = 0;
		footEnd_point[i].y = 0;
		footEnd_point[i].z = 0;
	}
}

/**
 * @brief  设置角度
 * @param  
 * @retval 无
 */
void robot_joint_set_angle(void)
{
	// 6条腿的电机
	for (uint8_t i=0; i < 6; i++) {
		
		// 一条腿3个电机
		for (uint8_t j=0; j < 3; j++) {
			motor_set_angle_single(i*3 + j, joint_ik[i].angle[j] / PI * 180.f);
		}
	}
}

/**
* @brief  机器人运动学逆解
 * @param  
 * @retval 无
 */
void robot_joint_ik(void)
{
	for (uint8_t i=0; i < 6; i++) {
		ik(&joint_ik[i], footEnd_point[i]);
	}
}

//float cnt_y = 1.5;
//float cnt_z = 0.5;

static void tset(void)
{
//	if (footEnd_point[0].y > 200) {
//		cnt_y = -3.5;
//	}
//	
//	if (footEnd_point[0].y < 70) {
//		cnt_y = 3.5;
//	}
//		
//	footEnd_point[0].y += cnt_y;
	
	
	
//	
//	if (footEnd_point[0].x > 60) {
//		cnt_z = -0.5;
//	}
//	
//	if (footEnd_point[0].x < -60) {
//		cnt_z = 1.5;
//	}
//		
//	footEnd_point[0].x += cnt_z;
}
/**
* @brief  机器人关节计算
 * @param  
 * @retval 无
 */
void robot_joint_math(void)
{

	tset();
	
	// 机器人运动学逆解
	robot_joint_ik();
	
	// 设置关节角度给实际机器人
	robot_joint_set_angle();
}
/******************* (C) COPYRIGHT 2025 FUYEBING*****END OF FILE************/
